//
// Created by xs on 24-3-16.
//
#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <tf/transform_datatypes.h>
#include "prometheus_msgs/UAVState.h"
#include "prometheus_msgs/UAVCommand.h"
#include "prometheus_msgs/UAVControlState.h"
#include "printf_utils.h"
#include <serial/serial.h>

#include "demo1/mission_utils.h"

class MySerial
{
public:
    MySerial(int baudrate, std::string port_name)
    {
        this->baudrate = baudrate;
        this->port_name = port_name; 
    }

    bool open_serial()
    {
        try
        {
            ser.setBaudrate(baudrate);
            ser.setPort(port_name);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
            ser.setTimeout(to); 
            ser.open(); 
        }
        catch(serial::IOException&  e)
        {
            std::cerr << e.what() << '\n';
            ROS_ERROR_STREAM("Unable to open port" << port_name); 
            return false;
        }
        
        if(ser.isOpen()) 
        { 
            ROS_INFO_STREAM("Serial Port initialized"); 
            return true;
        } 
        else 
        { 
            return false; 
        } 
        
    }

    // 0代表普通，1代表目标位置，2代表清除
    void write_pos(float pos_x, float pos_y, u_int8_t type)
    {
        if(!ser.isOpen())
        {
            ROS_WARN_STREAM("Serial not open, can not write data!");
            return;
        }
        data[0] = 0xFF;
        data[1] = type;
        data[2] = int(fabs(pos_x) * 50) > 255 ? 255: int(fabs(pos_x) * 50);
        data[3] = int(fabs(pos_y) * 50) > 255 ? 255: int(fabs(pos_y) * 50);
        data[4] = 0xFE;
        ser.write(data, 5);
    }

private:
    serial::Serial ser;
    u_int8_t data[5];
    std::string port_name;
    int baudrate;
};

using namespace std;
using namespace Eigen;
#define NODE_NAME "my_autonomous_landing"

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
bool g_use_pad_height; // 是否使用降落板绝对高度
float g_pad_height;
int g_uav_id;
std_msgs::Bool vision_switch;
float max_height; // 起始降落位置
float g_camera_offset[3];
prometheus_msgs::UAVCommand g_command_now;
prometheus_msgs::UAVControlState g_uavcontrol_state; // 遥控器状态
//---------------------------------------Drone---------------------------------------------
prometheus_msgs::UAVState g_UAVState; // 无人机状态
Eigen::Matrix3f g_R_Body_to_ENU;      // 无人机机体系至惯性系转换矩阵
//---------------------------------------Vision---------------------------------------------
nav_msgs::Odometry g_GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供）
Detection_result g_landpad_det;   // 检测结果
//---------------------------------------Track---------------------------------------------
float g_kp_land[3]; // 控制参数，控制无人机的速度

typedef Eigen::Vector2f Point;

float target_height;
float min_speed_moving_around, max_speed_moving_around;
float min_speed_tracking, max_speed_tracking;


// 状态机
enum EXEC_STATE
{
    WAITING,
    TAKEOFF,
    MOVINGAROUND,
    TRACKING,
    LOST,
    HOVERING,
    LANDING,
    RETURN2HOME
};
EXEC_STATE exec_state;

float g_distance_to_pad, g_distance_to_pad_without_z;
float g_arm_height_to_ground;
float g_arm_distance_to_pad;
string flying_points_file_path;
//---------------------------------------Output---------------------------------------------
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void landpadDetCb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
    g_landpad_det.object_name = "landpad";
    g_landpad_det.Detection_info = *msg;
    // 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    // 相机安装误差 在mission_utils.h中设置场
    // x, y轴交换
    g_landpad_det.pos_body_frame[0] = -g_landpad_det.Detection_info.position[1] + g_camera_offset[0];
    g_landpad_det.pos_body_frame[1] = -g_landpad_det.Detection_info.position[0] + g_camera_offset[1];
    g_landpad_det.pos_body_frame[2] = -g_landpad_det.Detection_info.position[2] + g_camera_offset[2];

    // 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
    g_landpad_det.pos_body_enu_frame = g_R_Body_to_ENU * g_landpad_det.pos_body_frame;
    // printf("g_R_Body_to_ENU Matrix: %f %f %f",g_R_Body_to_ENU[0], g_R_Body_to_ENU[1], g_R_Body_to_ENU[2]);
    

    if (g_use_pad_height)
    {
        //若已知降落板高度，则无需使用深度信息。
        g_landpad_det.pos_body_enu_frame[2] = g_pad_height - g_UAVState.position[2];
    }

    // 机体惯性系 -> 惯性系
    g_landpad_det.pos_enu_frame[0] = g_UAVState.position[0] + g_landpad_det.pos_body_enu_frame[0];
    g_landpad_det.pos_enu_frame[1] = g_UAVState.position[1] + g_landpad_det.pos_body_enu_frame[1];
    g_landpad_det.pos_enu_frame[2] = g_UAVState.position[2] + g_landpad_det.pos_body_enu_frame[2];
    // 此降落方案不考虑偏航角
    g_landpad_det.att_enu_frame[2] = 0.0;

    if (g_landpad_det.Detection_info.detected)
    {
        g_landpad_det.num_regain++;
        g_landpad_det.num_lost = 0;
    }
    else
    {
        g_landpad_det.num_regain = 0;
        g_landpad_det.num_lost++;
    }

    // 当连续一段时间无法检测到目标时，认定目标丢失
    if (g_landpad_det.num_lost > VISION_THRES)
    {
        g_landpad_det.is_detected = false;
    }

    // 当连续一段时间检测到目标时，认定目标得到
    if (g_landpad_det.num_regain > VISION_THRES)
    {
        g_landpad_det.is_detected = true;
    }
}

void droneStateCb(const prometheus_msgs::UAVState::ConstPtr &msg)
{
    g_UAVState = *msg;

    g_R_Body_to_ENU = get_rotation_matrix(g_UAVState.attitude[0], g_UAVState.attitude[1], g_UAVState.attitude[2]);
}

int baudrate;
string portName;

inline void readParams(const ros::NodeHandle &nh)
{
    nh.param<int>("uav_id", g_uav_id, 1);
    //强制上锁高度
    nh.param<float>("arm_height_to_ground", g_arm_height_to_ground, 0.4);
    //强制上锁距离
    nh.param<float>("arm_distance_to_pad", g_arm_distance_to_pad, 0.3);
    // 是否使用降落板绝对高度
    nh.param<bool>("use_pad_height", g_use_pad_height, false);
    nh.param<float>("pad_height", g_pad_height, 0.01);

    //追踪控制参数
    nh.param<float>("kpx_land", g_kp_land[0], 0.1);
    nh.param<float>("kpy_land", g_kp_land[1], 0.1);
    nh.param<float>("kpz_land", g_kp_land[2], 0.1);

    // 目标丢失时，最大到飞行高度，如果高于这个值判定为任务失败
    nh.param<float>("max_height", max_height, 3.0);

    // 相机安装偏移,规定为:相机在机体系(质心原点)的位置
    nh.param<float>("camera_offset_x", g_camera_offset[0], 0.0);
    nh.param<float>("camera_offset_y", g_camera_offset[1], 0.0);
    nh.param<float>("camera_offset_z", g_camera_offset[2], 0.0);

    // 不同模式下，无人机的最大，最小水平速度
    nh.param<float>("min_speed_moving_around", min_speed_moving_around, 0.1);
    nh.param<float>("max_speed_moving_around", max_speed_moving_around, 0.5);
    nh.param<float>("min_speed_tracking", min_speed_tracking, 0.05);
    nh.param<float>("max_speed_tracking", max_speed_tracking, 0.3);
    nh.param<float>("target_height", target_height, 0.5);
    nh.param<string>("flying_points_file_path", flying_points_file_path, "/home/xs/catkin_ws/src/demo1/config/flying_points.txt");

    // 串口参数
    nh.param<int>("baudrate", baudrate, 115200);
    nh.param<string>("portName", portName, "/dev/stm32");
}

inline void topicSub(ros::NodeHandle &nh)
{
    //【订阅】降落板与无人机的相对位置及相对偏航角  单位：米   单位：弧度
    //  方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    //  标志位：   detected 用作标志位 ture代表识别到目标 false代表丢失目标
    static ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/uav" + std::to_string(g_uav_id) + "/prometheus/object_detection/aruco_landpat", 10, landpadDetCb);

    // 无人机状态
    static ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::UAVState>("/uav" + std::to_string(g_uav_id) + "/prometheus/state", 10, droneStateCb);

    // 地面真值，此信息仅做比较使用 不强制要求提供
    static ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad", 10, [&](const nav_msgs::Odometry::ConstPtr &msg)
    { g_GroundTruth = *msg; });

    // 订阅遥控器状态
    static ros::Subscriber uav_control_state_sub = nh.subscribe<prometheus_msgs::UAVControlState>("/uav" + std::to_string(g_uav_id) + "/prometheus/control_state", 10, [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) -> void
    { g_uavcontrol_state = *msg; });
}

static ros::Publisher g_vision_switch_pub, g_command_pub, uav_setup_pub;

inline void topicAdv(ros::NodeHandle &nh)
{
    // 【发布】 视觉模块开关量
    g_vision_switch_pub = nh.advertise<std_msgs::Bool>("/uav" + std::to_string(g_uav_id) + "/prometheus/switch/landpad_det", 10);

    //【发布】发送给控制模块命令
    g_command_pub = nh.advertise<prometheus_msgs::UAVCommand>("/uav" + std::to_string(g_uav_id) + "/prometheus/command", 10);
    //创建无人机控制命令发布者
    uav_setup_pub = nh.advertise<prometheus_msgs::UAVSetup>("/uav" + std::to_string(g_uav_id) + "/prometheus/setup", 10);
}

float limit_vel(float vel, float min_vel, float max_vel)
{
    float return_vel = 0.0;
    if(fabs(vel) < min_vel)
    {
        return_vel = vel / fabs(vel) * min_vel;
    }
    else if(fabs(vel) > max_vel)
    {
        return_vel = vel / fabs(vel) * max_vel;
    }
    else
    {
        return_vel = vel;
    }

    return return_vel;
}

void print_params(vector<Point> &moving_points)
{
    //固定的浮点显示
    cout.setf(ios::fixed);
    // setprecision(n) 设显示小数精度为n位
    cout << setprecision(NUM_POINT);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout << GREEN << ">>>>>>>>>>>>>>>    Auto Landing Parameters     <<<<<<<<<<<<<<<" << TAIL << endl;
    cout << GREEN << "uav id                           : " << g_uav_id << TAIL << endl; 
    cout << GREEN << "Tracking factor [X Y Z]          : " << g_kp_land[0] << ", " << g_kp_land[1] << ", " << g_kp_land[2] << TAIL << endl; 
    cout << GREEN << "camera offset [X Y Z]            : " << g_camera_offset[0] << " [m] " << g_camera_offset[1] << " [m] " << g_camera_offset[2] << " [m]" << TAIL << endl; 
    cout << GREEN << "min/max speed when moving around : " << min_speed_moving_around << " [m/s] to " << max_speed_moving_around << " [m/s]" << TAIL << endl; 
    cout << GREEN << "min/max speed when tracking      : " << min_speed_tracking << " [m/s] to " << max_speed_tracking << " [m/s]" << TAIL << endl; 

    cout << GREEN << "target fly height                : " << target_height << " [m]" << TAIL << endl; 
    cout << GREEN << "flying_points_file_path          : " << flying_points_file_path << TAIL << endl;

    cout << GREEN << "baudrate & portName              : " << baudrate << "Bd & " << portName << TAIL << endl;

    cout << std::defaultfloat;
    cout.unsetf(ios::fixed);
    cout.unsetf(ios::left);
    cout.unsetf(ios::showpoint);
    cout.unsetf(ios::showpos);

    if(moving_points.size() > 0)
    {
        cout << GREEN << "Moving around points             : ";
        for(int i = 0; i < moving_points.size(); i++)
        {
            auto point = moving_points.at(i);
            if(i == moving_points.size() - 1) cout << "[" << point[0] << " " << point[1] << "].";
            else                              cout << "[" << point[0] << " " << point[1] << "], ";
        }
        cout << TAIL <<endl;
    }
}

// 文件不存在，文件格式不正确，文件内没有航点信息，都返回false
bool init_flying_points(string &path, vector<Point> &points)
{
    ifstream ifs;
    ifs.open(path.c_str(), ios::in);
 
    if (!ifs.is_open())
    {
        cout << "read fail." << endl;
        return false;
    }

    string buf;
    bool succeed_flag = true;
    while (getline(ifs, buf))
    {
        string::size_type pos = buf.find(",");
        if(pos != string::npos)
        {
            string s1 = buf.substr(0, pos);
            string s2 = buf.substr(pos + 1, buf.size());
            float pos_x = stof(s1);
            float pos_y = stof(s2);
            points.emplace_back(pos_x, pos_y);
            // cout << pos_x << " " << pos_x << endl;
        }
        else
        {
            succeed_flag = false;
            break;
        }
    }
    return succeed_flag;
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, NODE_NAME);
    ros::NodeHandle nh("~");

    // 节点运行频率： 30hz
    ros::Rate rate(30.0);

    // 读取配置参数
    readParams(nh);
    // 订阅话题
    topicSub(nh);
    // 发布话题
    topicAdv(nh);

    // 初始化巡逻区域
    vector<Point> moving_points;
    // 初始化航点
    bool res = init_flying_points(flying_points_file_path, moving_points);
    if(!res) 
    {
        ROS_ERROR_STREAM("can not get moving points data from " << flying_points_file_path << ".Program exiting!!");
        return 0;
    }

    print_params(moving_points);

    // 初始化串口
    MySerial myser(baudrate, portName);
    bool res_serial = myser.open_serial();
    if(!res_serial)
    {
        ROS_ERROR_STREAM("can't open serial at" << portName << " " << baudrate);
        // return 0;
    }

    unsigned int current_point_index = 0;
    Point current_point = moving_points[current_point_index];

    g_command_now.Command_ID = 1;
    exec_state = EXEC_STATE::WAITING;

    ROS_WARN("Please arm the vehicle and change control mode to COMMAND_CONTROL");

    prometheus_msgs::UAVSetup uav_setup;
    while (ros::ok())
    {
        //回调
        ros::spinOnce();

        switch (exec_state)
        {
            // 等待解锁+切换到COMMAND_CONTROL模式
            case WAITING:
            {
                if (g_uavcontrol_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL)
                {
                    ROS_INFO_STREAM("Entered COMMAND_CONTROL mode, progarm start running!!");

                    // 发送视觉节点启动指令
                    vision_switch.data = true;
                    g_vision_switch_pub.publish(vision_switch);
                    
                    exec_state = TAKEOFF;
                }       
                break;         
            }

            case TAKEOFF:
            {
                ROS_INFO_STREAM_ONCE("Enter take off MODE!");
                g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                g_command_now.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
                g_command_now.position_ref[0] = 0;
                g_command_now.position_ref[1] = 0;
                g_command_now.position_ref[2] = target_height;
                g_command_now.yaw_ref = 0.0;

                if(fabs(g_UAVState.position[2] - target_height) < 0.05)
                {
                    exec_state = MOVINGAROUND;
                }
                break;
            }

            case MOVINGAROUND:
            {
                ROS_INFO_STREAM_ONCE("Enter moving MODE!");
                if (g_landpad_det.is_detected)
                {
                    exec_state = TRACKING;
                    break;
                }

                if((fabs(g_UAVState.position[0] - current_point[0]) + fabs(g_UAVState.position[1] - current_point[1])) < 0.3)
                {
                    ROS_INFO_STREAM("Uav arrived at the " << current_point_index << "-th point: " << current_point[0] << ", " << current_point[1]);
                    // 悬停一段时间

                    current_point_index++;
                    if(current_point_index > moving_points.size() - 1) {
                        current_point_index = 0;
                    }
                    current_point = moving_points[current_point_index];
                    ROS_INFO_STREAM("Moving to Next point: " << current_point[0] << ", " << current_point[1]);
                }
                // Local坐标系目标点控制
                g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                g_command_now.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS;
                float vel_x = (current_point[0] - g_UAVState.position[0]) * g_kp_land[0];
                float vel_y = (current_point[1] - g_UAVState.position[1]) * g_kp_land[1];
                vel_x = limit_vel(vel_x, min_speed_moving_around, max_speed_moving_around);
                vel_y = limit_vel(vel_y, min_speed_moving_around, max_speed_moving_around);

                g_command_now.velocity_ref[0] = vel_x;
                g_command_now.velocity_ref[1] = vel_y;
                g_command_now.position_ref[2] = target_height;
                g_command_now.yaw_ref = 0.0;
                // PCOUT(1, GREEN, "Not Found Aruco marker, Moving around...");
                break;
            }
                // 追踪状态
            case TRACKING:
            {
                ROS_INFO_STREAM_ONCE("Enter tracking MODE!");
                // 正常追踪
                static int count_near = 0;
                char message_chars[256];
                sprintf(message_chars, "Tracking the Landing Pad, distance_to_the_pad :   %f [m] .", g_distance_to_pad_without_z);
                // 每0.2秒打印一次到pad的距离
                PCOUT(0.2, GREEN, message_chars);

                // 丢失,进入LOST状态
                if (!g_landpad_det.is_detected)
                {
                    exec_state = LOST;
                    PCOUT(0, YELLOW, "Lost the Landing Pad.");
                    break;
                }

                g_distance_to_pad_without_z = fabs(g_landpad_det.pos_body_frame[0]) + fabs(g_landpad_det.pos_body_frame[1]);

                // 到达目标附近，开始悬停
                // TODO 增加检测，持续一段时间后，再HOVERING
            
                if(g_distance_to_pad_without_z < 0.15)
                {
                    count_near ++;
                    if(count_near > 30)
                    {
                        exec_state = HOVERING;
                    }
                }
                else count_near = 0;


                // 机体系速度控制
                g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                g_command_now.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS; // xy velocity z position

                // 使用机体惯性系作为误差进行惯性系的速度控制
                for (int i = 0; i < 2; i++)
                {
                    g_command_now.velocity_ref[i] = limit_vel(g_kp_land[i] * g_landpad_det.pos_body_frame[i], min_speed_tracking, max_speed_tracking);
                }
                
                g_command_now.position_ref[2] = target_height;
                // 移动过程中，不调节航向角
                g_command_now.yaw_ref = 0.0;

                break;
            }
                // 目标丢失常识自动找回，在丢失目标后无人机先原定悬停一段时间，如果悬停到一段时候后
                // 仍然没有找到目标，则无人机持续向上飞行，到达一定高度后仍然未发现目标，判定自动
                // 降落任务失败，原点进入降落模式。
            case LOST:
            {
                ROS_INFO_STREAM_ONCE("Enter lost MODE!");
                static int lost_time = 0;
                lost_time++;

                // 悬停等待 尝试得到图像，如果获取不到，继续MOVING_AROUND
                if (lost_time < 10.0)
                {
                    g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover;

                    ros::Duration(0.3).sleep();
                }
                else
                {
                    exec_state = MOVINGAROUND;
                    lost_time = 0;
                }

                // 重新获得信息,进入TRACKING
                if (g_landpad_det.is_detected)
                {
                    exec_state = TRACKING;
                    PCOUT(0, GREEN, "Regain the Landing Pad.");
                }
                break;
            }

            case HOVERING:
            {
                ROS_INFO_STREAM_ONCE("Enter hovering MODE!");
                static bool hovered = false;
                if(!hovered)
                {
                    g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover;
                    hovered = true;
                    myser.write_pos(g_UAVState.position[0], g_UAVState.position[1], 1);
                }
                else
                {
                    bool res = whether2continue("To make uav return to home position?");
                    if(res) exec_state = RETURN2HOME;
                }
                break;
            }

            case RETURN2HOME:
            {
                ROS_INFO_STREAM_ONCE("Enter return to Home MODE!");
                Eigen::Vector3d uav_pos = {g_UAVState.position[0], g_UAVState.position[1], g_UAVState.position[2]};
                Eigen::Vector3d home_pos = {0.0, 0.0, target_height};
                float distance = (uav_pos - home_pos).norm();
                if(distance < 0.3)
                {
                    exec_state = LANDING;
                }
                else
                {
                    g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                    g_command_now.Move_mode = prometheus_msgs::UAVCommand::XYZ_VEL;
                    g_command_now.velocity_ref[0] = limit_vel((home_pos[0] - uav_pos[0]) * g_kp_land[0], 0.1, 0.5);
                    g_command_now.velocity_ref[1] = limit_vel((home_pos[1] - uav_pos[1]) * g_kp_land[1], 0.1, 0.5);
                    g_command_now.velocity_ref[2] = limit_vel((home_pos[2] - uav_pos[2]) * g_kp_land[2], 0, 0.5);
                }
                break;
            }

            case LANDING:
            {
                ROS_INFO_STREAM_ONCE("Enter LANDING MODE!");
                g_command_now.Agent_CMD = prometheus_msgs::UAVCommand::Land;
                break;
            }
        }
        g_command_now.header.stamp = ros::Time::now();
        g_command_now.Command_ID = g_command_now.Command_ID + 1;
        g_command_pub.publish(g_command_now);
        rate.sleep();

        if(exec_state == LANDING && !g_UAVState.armed) break;
    }

    return 0;
}